/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package xenon3d.vector;

/**
 * A 4 element unit quaternion represented by single precision floating
 * point x,y,z,w coordinates. The quaternion must always be normalized.
 * @author Volker Everts
 * @version 0.1 - 18.09.2011: Created
 */
public class Quat4f extends Tuple4f {

    // <editor-fold defaultstate="collapsed" desc=" Static Attributes ">

    /** The serial version UID. */
    private static final long serialVersionUID = 20110918L;

    // </editor-fold>

    // <editor-fold defaultstate="collapsed" desc=" Initialization ">

    /**
     * Constructs and initializes a Quat4f to (0,0,0,0).
     */
    public Quat4f() {
        super();
    }

    /**
     * Constructs and initializes a Quat4f from the specified xyzw coordinates.
     * @param x the x coordinate
     * @param y the y coordinate
     * @param z the z coordinate
     * @param w the w scalar component
     */
    public Quat4f(float x, float y, float z, float w) {
        super(x, y, z, w);
    }

    /**
     * Constructs and initializes a Quat4f from the specified other Quat4f.
     * @param t the tuple containing the initialization x y z w data
     */
    public Quat4f(Tuple4f t) {
        set(t);
    }

    // </editor-fold>

    // <editor-fold defaultstate="collapsed" desc=" Public Properties (Conversions)">

    /**
     * Sets the value of this quaternion to the normalized rotational equivalent
     * of the specified euler-angles.<p>
     * Equations<p>
     * <code>
     * Equations
     * w = c1 c2 c3 - s1 s2 s3
     * x = s1 s2 c3 + c1 c2 s3
     * y = s1 c2 c3 + c1 s2 s3
     * z = c1 s2 c3 - s1 c2 s3
     * where:
     * c1 = cos(heading / 2)
     * c2 = cos(attitude / 2)
     * c3 = cos(bank / 2)
     * s1 = sin(heading / 2)
     * s2 = sin(attitude / 2)
     * s3 = sin(bank / 2)
     * </code>
     * @param e the EulerAngle3f
     */
    public final void set(EulerAngle3f e) {
        float c1 = FMath.cos(e.y * 0.5f);
        float s1 = FMath.sin(e.y * 0.5f);
        float c2 = FMath.cos(e.x * 0.5f);
        float s2 = FMath.sin(e.x * 0.5f);
        float c3 = FMath.cos(e.z * 0.5f);
        float s3 = FMath.sin(e.z * 0.5f);
  	x = c1 * c2 * s3 + s1 * s2 * c3;
	y = s1 * c2 * c3 + c1 * s2 * s3;
	z = c1 * s2 * c3 - s1 * c2 * s3;
        w = c1 * c2 * c3 - s1 * s2 * s3;
    }

    /**
     * Sets the value of this quaternion to the equivalent rotation
     * of the AxisAngle argument.<p>
     * Equations<p>
     * <code>
     * qx = ax * sin(angle/2)
     * qy = ay * sin(angle/2)
     * qz = az * sin(angle/2)
     * qw = cos(angle/2)
     * the axis is normalised so:
     * ax*ax + ay*ay + az*az = 1
     * the quaternion is also normalised so:
     * cos(angle/2)2 + ax*ax * sin(angle/2)2 + ay*ay * sin(angle/2)2+ az*az * sin(angle/2)2 = 1
     * </code>
     * @param a the AxisAngle4f
     */
    public final void set(AxisAngle4f a) {
        float mag = a.normSq();
        if (mag > FMath.EPS) {
            float s = FMath.sin(a.w * 0.5f);
            if (!FMath.isEqual(mag, 1.0f, FMath.EPS)) s /= FMath.sqrt(mag);
            x = a.x * s;
            y = a.y * s;
            z = a.z * s;
            w = FMath.cos(a.w * 0.5f);
        }
        else set(0.0f, 0.0f, 0.0f, 0.0f);
    }

    /**
     * Sets the value of this quaternion to the normalized rotational component
     * of the passed rotation matrix.<p>
     * Equations<p>
     * <code>
     * if:
     * the matrix is orthoginal (det(matrix)=1)
     * trace = 4 - 4*qx2 - 4*qy2 - 4*qz2
     *       = 4 (1 -qx2 - qy2 - qz2)
     *       = m00 + m11 + m22 + 1
     * If the trace of the matrix is greater than zero, then the result is:
     * s = 0.5 / sqrt(trace)
     * w = 0.25 / s
     * x = (m21 - m12) * s
     * y = (m02 - m20) * s
     * z = (m10 - m01) * s
     * <code>
     * @param mat the Matrix3f
     */
    public final void set(Matrix3f mat) {
        float t = mat.m11 + mat.m22 + mat.m33;
        if (t > 0.0f) {
            float s = FMath.sqrt(t + 1.0f) * 2.0f;
            w = 0.25f * s;
            x = (mat.m32 - mat.m23) / s;
            y = (mat.m13 - mat.m31) / s;
            z = (mat.m21 - mat.m12) / s;
        }
        else if ((mat.m11 > mat.m22) && (mat.m11 > mat.m33)) {
            float s = FMath.sqrt(1.0f + mat.m11 - mat.m22 - mat.m33) * 2.0f;
            w = (mat.m32 - mat.m23) / s;
            x = 0.25f * s;
            y = (mat.m12 + mat.m21) / s;
            z = (mat.m13 + mat.m31) / s;
        }
        else if (mat.m22 > mat.m33) {
            float s = FMath.sqrt(1.0f + mat.m22 - mat.m11 - mat.m33) * 2.0f;
            w = (mat.m13 - mat.m31) / s;
            x = (mat.m12 + mat.m21) / s;
            y = 0.25f * s;
            z = (mat.m23 + mat.m32) / s;
        }
        else {
            float s = FMath.sqrt(1.0f + mat.m33 - mat.m11 - mat.m22) * 2.0f;
            w = (mat.m21 - mat.m12) / s;
            x = (mat.m13 + mat.m31) / s;
            y = (mat.m23 + mat.m32) / s;
            z = 0.25f * s;
        }
    }

    /**
     * Sets the value of this quaternion to the normalized rotational component
     * of the passed transformation matrix.
     * @param mat the Matrix4f
     */
    public final void set(Matrix4f mat) {
        float t = mat.m11 + mat.m22 + mat.m33;
        if (t > 0.0f) {
            float s = FMath.sqrt(t + 1.0f) * 2.0f;
            w = 0.25f * s;
            x = (mat.m32 - mat.m23) / s;
            y = (mat.m13 - mat.m31) / s;
            z = (mat.m21 - mat.m12) / s;
        }
        else if ((mat.m11 > mat.m22) && (mat.m11 > mat.m33)) {
            float s = FMath.sqrt(1.0f + mat.m11 - mat.m22 - mat.m33) * 2.0f;
            w = (mat.m32 - mat.m23) / s;
            x = 0.25f * s;
            y = (mat.m12 + mat.m21) / s;
            z = (mat.m13 + mat.m31) / s;
        }
        else if (mat.m22 > mat.m33) {
            float s = FMath.sqrt(1.0f + mat.m22 - mat.m11 - mat.m33) * 2.0f;
            w = (mat.m13 - mat.m31) / s;
            x = (mat.m12 + mat.m21) / s;
            y = 0.25f * s;
            z = (mat.m23 + mat.m32) / s;
        }
        else {
            float s = FMath.sqrt(1.0f + mat.m33 - mat.m11 - mat.m22) * 2.0f;
            w = (mat.m21 - mat.m12) / s;
            x = (mat.m13 + mat.m31) / s;
            y = (mat.m23 + mat.m32) / s;
            z = 0.25f * s;
        }
    }

    // </editor-fold>

    // <editor-fold defaultstate="collapsed" desc=" Public Methods ">

    /**
     * Sets the value of this quaternion to the conjugate of itself.
     */
    public final void conjugate() {
        x = -x;
        y = -y;
        z = -z;
    }

    /**
     * Sets the value of this quaternion to the conjugate of another quaternion q.
     * @param q the other quaternion
     */
    public final void conjugate(Quat4f q) {
        this.x = -q.x;
        this.y = -q.y;
        this.z = -q.z;
        this.w =  q.w;
    }

    /**
     * Sets the value of this quaternion to the quaternion inverse of itself.
     */
    public final void invert() {
        x = -x;
        y = -y;
        z = -z;
        w = -w;
    }

    /**
     * Sets the value of this quaternion to quaternion inverse of another
     * quaternion q.
     * @param q the other quaternion
     */
    public final void invert(Quat4f q) {
        set(q);
        invert();
    }

    /**
     * Returns the norm of this quaternion.
     * @return the quaternion norm
     */
    public final float norm() {
        return FMath.sqrt(normSq());
    }

    /**
     * Returns the squared norm of this quaternion.
     * @return the squared norm.
     */
    public final float normSq() {
        return x * x + y * y + z * z + w * w;
    }

    /**
     * Normalizes the value of this quaternion in place.
     */
    public final void normalize() {
        float norm = normSq();
        if (FMath.isEqual(norm, 1.0f, FMath.EPS)) return;
        if (FMath.isZero(norm, FMath.EPS)) set(0.0f, 0.0f, 0.0f, 1.0f);
        else {
            norm = FMath.sqrt(norm);
            x /= norm;
            y /= norm;
            z /= norm;
            w /= norm;
        }
    }

    /**
     * Sets the value of this quaternion to the normalized value of another
     * quaternion q.
     * @param q the other quaternion
     */
    public final void normalize(Quat4f q) {
        set(q);
        normalize();
    }

    /**
     * Sets the value of this quaternion to the quaternion product of itself and
     * another quaternion q (this = this * q).
     * @param q the other quaternion
     */
    public final void mul(Quat4f q) {
        mul(this, q);
    }

    /**
     * Sets the value of this quaternion to the quaternion product of quaternions
     * q1 and q2 (this = q1 * q2). Note that this is safe for aliasing (e.g. this
     * can be q1 or q2).
     * @param q1 the first quaternion
     * @param q2 the second quaternion
     */
    public final void mul(Quat4f q1, Quat4f q2) {
        float a = q1.w * q2.x + q2.w * q1.x + q1.y * q2.z - q1.z * q2.y;
        float b = q1.w * q2.y + q2.w * q1.y - q1.x * q2.z + q1.z * q2.x;
        float c = q1.w * q2.z + q2.w * q1.z + q1.x * q2.y - q1.y * q2.x;
        float d = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
        x = a;
        y = b;
        z = c;
        w = d;
    }

    /**
     * Multiplies quaternion q1 by the inverse of quaternion q2 and places
     * the value into this quaternion.  The value of both argument quaternions
     * is preservered (this = q1 * q2^-1).
     * @param q1 the first quaternion
     * @param q2 the second quaternion
     */
    public final void mulInverse(Quat4f q1, Quat4f q2) {
        Quat4f temp = new Quat4f(q2);
        temp.invert();
        mul(q1, temp);
    }

    /**
     * Multiplies this quaternion by the inverse of another quaternion q. The
     * value of the argument quaternion is preserved (this = this * q^-1).
     * @param q the other quaternion
     */
    public final void mulInverse(Quat4f q) {
        Quat4f temp = new Quat4f(q);
        temp.invert();
        mul(this, temp);
    }

    /**
     * Performs a great circle interpolation between this quaternion and
     * another quaternion and places the result into this quaternion.
     * @param q the other quaternion
     * @param d the interpolation weight, must be in the range 0 <= d <= 1
     */
    public final void interpolate(Quat4f q, float d) {
        interpolate(this, q, d);
    }

    /**
     * Performs a great circle interpolation between quaternions q1 and q2 and
     * places the result into this quaternion.
     * @param q1  the first quaternion
     * @param q2  the second quaternion
     * @param d the interpolation weight, must be in the range 0 <= d <= 1
     */
    public final void interpolate(Quat4f q1, Quat4f q2, float d) {
        float dot = q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w;
        float om = FMath.acos(dot);
        float sn = FMath.sin(om);
        float s1 = FMath.sin((1.0f - d) * om) / sn;
        float s2 = FMath.sin(d * om) / sn;
        x = s1 * x + s2 * q1.x;
        y = s1 * y + s2 * q1.y;
        z = s1 * z + s2 * q1.z;
        w = s1 * w + s2 * q1.w;
    }

    // </editor-fold>

} // end class Quat4f